
#include "body.h"

#include <iostream>

#include "vector4.h"
#include "matrix44.h"

Body::Body(void) : Object()
{
	id = -1;
	mass = 0;
	MOI = Matrix44();
	MOIinv = MOI.inverse();
	impulse = Vector4();
	ori = Vector4();
	
}

void Body::update(float etime)
{	
	vel = vel + impulse;
	if (pos[1] > 10.0){
		Vector4 forceVal = Vector4(0.0f,-9.8f,0.f);
		forceVal.scale(mass);
		applyForce(Force(pos[0],pos[1],pos[2], forceVal));
	}
	Object::update(etime);
	vel = vel - impulse;
	impulse = Vector4();
	//std::cout << "acc " << acc[0] << " " << acc[1] << " " << acc[2] << std::endl;
	
}

void Body::transformXYZHRPS(float x, float y, float z, float h, float p, float r, float s){
	//tMat = tMat.transformXYZHPRS(x, y, z, h, p, r, s);
	translateXYZ(x,y,z);
	rotHPR(h,p,r);
}

void Body::rotHPR(float h, float p, float r){
	ori = ori + Vector4(h,p,r);
	//std::cout << "ori " << ori[0] << " " << ori[1] << " " << ori[2] << std::endl;
}

Vector4 Body::getTorque(Force forceApplied){
	Vector4 colPos = forceApplied.getPos();
	Vector4 colVector = colPos-pos;
	
	Vector4 torque = colVector.cross(forceApplied.getForce());

	return torque;

	
}

void Body::applyForce(Force forceApplied){
	Vector4 posForce = forceApplied.getForce();
	posForce.scale(1.0f/mass);
	acc = acc+ posForce;
	posForce.scale(1.f/100.0);
	Force aForce = Force(forceApplied.pos[0],forceApplied.pos[1],forceApplied.pos[2], posForce);
	aAcc = aAcc + (MOIinv*getTorque(aForce));
}

void Body::updateMatrix(void){
	tMat = Matrix44().transformXYZHPRS(pos[0], pos[1], pos[2], ori[0], ori[1], ori[2], sc);
}